/*
@2024-12-06: 增加
@2023-12-04: 更新通过UDP socket给ICS返回消息的功能
@2024-12-03: 增加动态配置读出速度功能，在改变读出速度的同时，还需要更新权重系数；删除部分旧代码
@2024-11-29: 改用UDP
@2024-11-20: 将PL_isMaster功能集成到
@2024-10-23: 基于单芯片相机的socket服务端程序多线程版适配修改后用于2X2拼接相机。该服务端程序运行于4个zynq
             ps端，作为后台程序

TODO:
-[ ] 增加挂载Linux主机工作目录的功能。要实现该功能，需要ICS那边将Linux主机待挂载的目录发送给PS端。 


*/

#include "MephistoCAM.h"

#define PORT            9527

#define IP_LINUX_HOST   "192.168.0.233"

#define IP_ICS          "192.168.0.233"
#define PORT_ICS        10100

// #define LISTEN_QUEUE    16
#define BUFFER_SIZE     2048

typedef struct _SockInfo {
    struct sockaddr_in  *server_sockaddr;
    struct sockaddr_in  *server_addr_ics;
    int                 sock_from_ics;
    int                 sock_to_ics;
    MephistoCAM_Config  *mcfg;
} SockInfo;


////////////////////////////////////////////////////////////////////////////////////
//  全局变量
int ccd_idx;
char myName[64];                // 保存每个CCD的名称blue{0,1,2,3}
char binfilename[1024];         // 保存bin文件的文件名（含相对路径）

SockInfo sockinfo_config;
MephistoCAM_Config MCAM_Config;

struct sockaddr_in client_addr; 
socklen_t client_addr_len;

char logfile[1024];
FILE *fp_log;
////////////////////////////////////////////////////////////////////////////////////

int extractCommand( const char sock_msg[1024], char *operator, char *operand );

void *server_ddr( void *arg );

void *server_for_ics(void *arg );

////////////////////////////////////////////////////////////////////////////////////
// 状态参数
// static int state_reading;           // 0: not reading; 1: reading
// static int linux_host_is_mounted;   // 0: not mounted; 1: mounted

////////////////////////////////////////////////////////////////////////////////////

int main( int argc, char *argv[] ) {

    if( argc < 3 ){
        printf("\n*********************************************\n");
        printf("usage: %s band ccd_idx\n", argv[0]);
        printf("*********************************************\n");
        return 0;
    }
    
    ccd_idx = atoi(argv[2]);
    sprintf(myName, "%s%d", argv[1], ccd_idx);
    fprintf(stdout, "# My Name is : %s\n", myName);
    sprintf(binfilename, "/mnt/%s.bin", myName);
    sprintf(logfile, "/mnt/debug_blue%d.txt", ccd_idx);
    fprintf(stdout, "# binfilename: %s\n", binfilename);

    fp_log = fopen(logfile, "a+");
    
    //=================================================
    //  导入所有配置参数
    //=================================================
    char config_dir[1024];      // 保存参数配置文件所在的目录(相对路径)
    char config_dir_tmp[512];

    FILE *fp_wkdir = fopen("/sd/WKDIR", "r");
    fscanf(fp_wkdir, "%s", config_dir_tmp);
    fclose(fp_wkdir);

    sprintf(config_dir, "%s_%s", config_dir_tmp, myName);

    init_MephistoCAM_Config( &MCAM_Config, config_dir );


    //=========================================================================
    // GPIO 功能定义
    // uint32_t *gpio_flt_ctrl;        // ADDR_Float_Point_CTRL
    // uint32_t *gpio_flt_data;        // ADDR_Float_Point_DATA
    // uint32_t *gpio_in;              // ADDR_GPIO_IN
    // uint32_t *gpio_pl_ps;           // ADDR_PL_PS_DDR_CNT
    // uint32_t *gpio_out;             // ADDR_GPIO_OUT
    // uint32_t *gpio_out2;            // ADDR_GPIO_OUT
    // uint32_t *gpio_spi_ps_ctrl;     // ADDR_GPIO_SPI_PS_CTRL
    // uint32_t *gpio_spi_ps_data;     // ADDR_GPIO_SPI_PS_DATA


    int fd_gpio = open("/dev/mem", O_RDWR | O_SYNC);

    uint32_t *gpio_flt_ctrl     = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_Float_Point_CTRL);
    uint32_t *gpio_flt_data     = gpio_flt_ctrl + 2;

    uint32_t *gpio_in           = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_IN);
    uint32_t *gpio_pl_ps        = gpio_in + 2;

    uint32_t *gpio_out          = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_OUT);
    uint32_t *gpio_out2         = gpio_out + 2;

    uint32_t *gpio_spi_ps_ctrl  = mmap(NULL, 10, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, ADDR_GPIO_SPI_PS_CTRL);
    uint32_t *gpio_spi_ps_data  = gpio_spi_ps_ctrl + 2;

    MCAM_Config.gpio_flt_ctrl   = gpio_flt_ctrl;
    MCAM_Config.gpio_flt_data   = gpio_flt_data;
    MCAM_Config.gpio_in         = gpio_in;
    MCAM_Config.gpio_pl_ps      = gpio_pl_ps;
    MCAM_Config.gpio_out        = gpio_out;
    MCAM_Config.gpio_out2       = gpio_out2;
    MCAM_Config.gpio_spi_ps_ctrl= gpio_spi_ps_ctrl;
    MCAM_Config.gpio_spi_ps_data= gpio_spi_ps_data;

    //=================================================
    //      SOCKET
    //=================================================
    
    bzero( &client_addr, sizeof(client_addr) );
    client_addr_len = sizeof(client_addr);

    ////////////////////////////////////////////////////////////////////////
    // 创建和初始化从ICS接受指令的 UDP socket
    // int sock_fd = socket(AF_INET,SOCK_STREAM,0);        // TCP
    int on = 1;                                         // Enable address reuse
    int sock_fd = socket(AF_INET,SOCK_DGRAM,0);        // UDP
    setsockopt( sock_fd, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) );
    
    int sock_from_ics = socket(AF_INET, SOCK_DGRAM, 0);
    setsockopt( sock_from_ics, SOL_SOCKET, SO_REUSEADDR, &on, sizeof(on) );

    struct sockaddr_in server_sockaddr;                 // socket service
    // struct sockaddr_in client_addr;                     // socket client

    // socklen_t client_addr_len = sizeof(client_addr);

    memset( &server_sockaddr, 0, sizeof(server_sockaddr));
    // bzero( &client_addr, sizeof(client_addr) );

    server_sockaddr.sin_family      = AF_INET;
    server_sockaddr.sin_port        = htons(PORT);
    server_sockaddr.sin_addr.s_addr = htonl(INADDR_ANY);

    ///bind成功返回0出错返回-1
    if( bind(sock_from_ics, (struct sockaddr *)&server_sockaddr, sizeof(server_sockaddr)) == -1 )
    {
        fprintf(stderr, "[Mephisto_BLUE_CAM] Failed to bind socket, quit.\n");
        exit(-1);
    }

    ////////////////////////////////////////////////////////////////////////
    // 创建和初始化给ICS发送回复的UDP套接字,
    int sock2ics;
    if( (sock2ics = socket(AF_INET,SOCK_DGRAM,0)) < 0 ){
        perror("**** failed creating sock2ics ****\n");
        exit(EXIT_FAILURE);
    }

    // 配置服务端地址
    struct sockaddr_in server_addr_ics;                // 用于给ICS返回消息
    memset(&server_addr_ics, 0, sizeof(server_addr_ics));
    server_addr_ics.sin_family  = AF_INET;
    server_addr_ics.sin_port    = htons(PORT_ICS);

    // 将IP地址转换为网络字节序
    if( inet_pton(AF_INET, IP_ICS, &server_addr_ics.sin_addr) <= 0 ){
        perror("**** failed converting ICS address ****\n");
        close(sock2ics);
        exit(EXIT_FAILURE);
    }
    ////////////////////////////////////////////////////////////////////////

    sockinfo_config.server_sockaddr = &server_sockaddr;
    sockinfo_config.sock_from_ics   = sock_from_ics;

    sockinfo_config.server_addr_ics = &server_addr_ics;
    sockinfo_config.sock_to_ics     = sock2ics;

    sockinfo_config.mcfg            = &MCAM_Config;
    

    // 创建从DDR取出数据的线程
    pthread_t tid_server_ddr;
    pthread_create(&tid_server_ddr, NULL, server_ddr, &sockinfo_config);
    pthread_detach(tid_server_ddr);

    // 创建接受并处理从ICS接收到的指令的线程
    pthread_t tid_server_ics;
    pthread_create(&tid_server_ics, NULL, server_for_ics, &sockinfo_config);
    pthread_detach(tid_server_ics);

    while(1){
        sleep(1);
    }

    close(sock_fd);

    return 0;
}


int extractCommand( const char sock_msg[1024], char *operator, char *operand ){
    char local_str[1024];
    strcpy(local_str, sock_msg);
    
    char *p;
    sprintf(operator, "%s", strtok(local_str, "|"));
    // fprintf(stdout, "##> operator: --%s--\n", operator);
    
    p = strtok(NULL, "|");

    if( p != NULL ){
        sprintf(operand, "%s", p);
        // fprintf(stdout, "##> operand:  --%s--\n", operand);
    }

    return 0;
}


void *server_ddr( void *arg ){

    fprintf(fp_log, "> server_ddr thread started ...\n");

    
    char sendbuf[BUFFER_SIZE];

    SockInfo *sockinfo_ptr = (SockInfo *)arg;

    MephistoCAM_Config *mcfg = sockinfo_ptr->mcfg;

    int fd_gpio = open("/dev/mem", O_RDWR | O_SYNC);

    //指向DDR固定地址的指针
    uint32_t base_addr = 0x20000000;    // 512MByte 特殊地址值！！！！
    uint32_t n_col = mcfg->config_ccd_drive->htrans_num;
    uint32_t n_row = mcfg->config_ccd_drive->vtrans_num;
    uint32_t n_bytes = 2;
    uint32_t mem_size_in_Bytes = n_col*16*n_bytes;   // the actual number of Bytes of a row
    uint32_t *mem_ptr = mmap(NULL, n_row*n_col*16*n_bytes, PROT_READ | PROT_WRITE, MAP_SHARED, fd_gpio, base_addr);
    uint32_t *ptr;

    uint32_t row_cnt = 4700;    // row_cnt的默认值不能为0，防止PL端读出状态机静止时BIT_DDR_DATA_RDY=1

    while(1) {

        if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) && (row_cnt==0) ){

            fprintf(fp_log, "# start transfering DDR data into binary...\n");
            fprintf(stdout, "[server_ddr]# start transfering DDR data into binary...\n");

            ptr = mem_ptr;
            FILE *fb = NULL;
            fb = fopen(binfilename, "wba");   // append mode
            do {
                if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) ){
                    fwrite(ptr, sizeof(unsigned char), mem_size_in_Bytes, fb);
                    ptr += n_col*n_bytes*4;
                    row_cnt++;
                }
            } while( row_cnt < n_row );

            if( fb != NULL )
                fclose(fb);

            sprintf(sendbuf, "readoutdone%d", ccd_idx);
            fprintf(fp_log, "> ready to send readoutdone%d to ICS\n", ccd_idx);
            fprintf(stdout, "[server_ddr]> ready to send readoutdone%d to ICS\n", ccd_idx);

/*if( sendto(sockinfo_ptr->sock_to_ics, sendbuf, strlen(sendbuf), 0, (struct sockaddr*)(sockinfo_ptr->server_addr_ics), sizeof(*(sockinfo_ptr->server_addr_ics))) < 0 )*/

if( sendto(sockinfo_ptr->sock_to_ics, sendbuf, strlen(sendbuf), 0, (struct sockaddr*)(sockinfo_ptr->server_addr_ics), sizeof(*(sockinfo_ptr->server_addr_ics))) < 0 )
            {
                fprintf(stdout, "[server_ddr]**** failed sending %s to ICS\n", sendbuf);
                close(sockinfo_ptr->sock_to_ics);
                exit(EXIT_FAILURE);
            } else {
                fprintf(stdout, "[server_ddr]>>>> sucessfully send %s to ICS\n", sendbuf);
            }

            fprintf(fp_log, "> finished sending readoutdone%d to ICS\n", ccd_idx);
            fprintf(stdout, "[server_ddr]> finished sending readoutdone%d to ICS\n", ccd_idx);
        }

        // 检测到BIT_DDR_DATA_RDY变为0时，表示FPGA端的读出状态机再次启动，此时将row_cnt复位至0
        if( getbit( *(mcfg->gpio_in), BIT_DDR_DATA_RDY) == 0 ){
            row_cnt = 0;
        }

        usleep(100);    // 100us睡眠，避免高CPU占用
    }
}

void *server_for_ics( void *arg ){

    char recvbuf[BUFFER_SIZE];
    char sendbuf[BUFFER_SIZE];
    char operator[128]   = "";
    char operand[256]    = "";

    SockInfo *sockinfo_ptr = (SockInfo *)arg;

    int cmd_set = 0;
    while(1) {

        cmd_set = 0;
        bzero(recvbuf, sizeof(recvbuf));
        bzero(sendbuf, sizeof(sendbuf));
        bzero(operator, sizeof(operator));
        bzero(operand, sizeof(operand));

        socklen_t len = recvfrom(sockinfo_ptr->sock_from_ics, recvbuf, sizeof(recvbuf), 
                                0, (struct sockaddr*)&client_addr, &client_addr_len);

        if( len > 0 ){

            fprintf(stdout, "\n**** UDP socket recieved: %s\n", recvbuf);

            extractCommand( recvbuf, operator, operand );

            if( strcmp(operator, "speed") == 0 ){

                fprintf(fp_log, "> ready to configure readout speed ICS\n");

                // 先通过system系统调用函数直接调用固定路径下的脚本完成配置任务
                char cmd1[1024];
                char cmd2[1024];

                if( strcmp(operand, "250k") == 0 ){
                    fprintf(fp_log, "# chosen 250kHz\n");
                    fprintf(stdout, "# chosen 250kHz\n");

                    sprintf(cmd1, "bin/ccd290_drive_config params_250K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "bin/dcds_w_cfg params_250K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    
                    cmd_set = 1;
                }

                if( strcmp(operand, "500k") == 0 ){
                    fprintf(fp_log, "# chosen 500kHz\n");
                    fprintf(stdout, "# chosen 250kHz\n");

                    sprintf(cmd1, "bin/ccd290_drive_config params_500K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "bin/dcds_w_cfg params_500K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    
                    cmd_set = 1;
                }

                if( strcmp(operand, "1000k") == 0 ){
                    fprintf(fp_log, "# chosen 1000kHz\n");
                    fprintf(stdout, "# chosen 250kHz\n");

                    sprintf(cmd1, "bin/ccd290_drive_config params_900K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "bin/dcds_w_cfg params_900K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    
                    cmd_set = 1;
                }

                if( strcmp(operand, "2000k") == 0 ){
                    fprintf(fp_log, "# chosen 2000kHz\n");
                    fprintf(stdout, "# chosen 250kHz\n");

                    sprintf(cmd1, "bin/ccd290_drive_config params_2000K_mosaic_blue%d/ccd_drv.ini", ccd_idx);
                    sprintf(cmd2, "bin/dcds_w_cfg params_2000K_mosaic_blue%d/dcds_weights_cfg.ini", ccd_idx);
                    
                    cmd_set = 1;
                }

                if( cmd_set ){
                    system(cmd1);
                    usleep(10000);
                    system(cmd2);
                }

                memset(sendbuf, 0, sizeof(sendbuf));
                sprintf(sendbuf, "speeddone%d", ccd_idx);
                
                fprintf(fp_log, "> ready to send speeddone%d to ICS\n", ccd_idx);

                if( sendto(sockinfo_ptr->sock_to_ics, sendbuf, strlen(sendbuf), 0, 
                    (struct sockaddr*)(sockinfo_ptr->server_addr_ics), 
                    sizeof(*(sockinfo_ptr->server_addr_ics))) < 0 ){
                    printf("**** failed sending %s to ICS\n", sendbuf);
                    close(sockinfo_ptr->sock_to_ics);
                    exit(EXIT_FAILURE);
                } else {
                    printf(">>>> sucessfully send %s to ICS\n", sendbuf);
                }

                fprintf(fp_log, "> finished sending speeddone%d to ICS\n", ccd_idx);
                fprintf(stdout, "> finished sending speeddone%d to ICS\n", ccd_idx);

                cmd_set = 0;
            }
            // else if( strcmp(operator, "row_size") == 0 ){
            //     printf("setting row_size to: %s\n", operand);
            // }
            // else if( strcmp(operator, "col_size") == 0 ){
            //     printf("setting col_size to: %s\n", operand);
            // }
            else if( strcmp(operator, "synctime") == 0 ){
                // 更新同步PS端系统时间
                time_t arm_sys_time = atoi(operand);
                struct timeval tv;
                tv.tv_sec = arm_sys_time;//+8*3600;
                tv.tv_usec= 0;
                settimeofday(&tv, NULL);
                time_t t_now = time(NULL);
                fprintf(stdout, "> ps sys time %s\n", ctime(&t_now));
            }
            else if( strcmp(operator, "readout") == 0 ){ // for debug
                printf("received readout\n");
                memset(sendbuf, 0, sizeof(sendbuf));
                sprintf(sendbuf, "readoutdone%d", ccd_idx);
                if( sendto(sockinfo_ptr->sock_to_ics, sendbuf, strlen(sendbuf), 0, 
                    (struct sockaddr*)(sockinfo_ptr->server_addr_ics), 
                    sizeof(*(sockinfo_ptr->server_addr_ics))) < 0 ){
                    printf("**** failed sending %s to ICS\n", sendbuf);
                    close(sockinfo_ptr->sock_to_ics);
                    exit(EXIT_FAILURE);
                } else {
                    printf(">>>> sucessfully send %s to ICS\n", sendbuf);
                }
            }
            else {
                fprintf(stdout, "\n***** unresolved command: %s ******\n", operator);
            }

            memset(recvbuf, 0,sizeof(recvbuf));
            memset(sendbuf, 0, sizeof(sendbuf));
            memset(operator, 0, sizeof(operator));
            memset(operand, 0, sizeof(operand));
        }
        // usleep(1000);   // 1ms sleep
    }
}
